#include "moveit/rviz_plugin_scene_editor/scene_editor_frame.h"
#include "moveit/rviz_plugin_scene_editor/scene_editor_display.h"
#include "ui_rviz_plugin_scene_editor_frame.h"

#include <QMessageBox>
#include <QFileDialog>
#include <QInputDialog>
#include <QShortcut>
#include <QTimer>
#include <QMenu>

#include <geometric_shapes/shape_operations.h>
#include <tf2_eigen/tf2_eigen.h>
#include <moveit/robot_interaction/interactive_marker_helpers.h>
#include <interactive_markers/tools.h>

#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>

static const double MIN_VAL = 1e-6;

namespace fs = boost::filesystem;

namespace
{
bool isDirExist(const std::string& path_name)
{
  if (boost::filesystem::exists(path_name) && boost::filesystem::is_directory(path_name))
  {
    return true;
  }
  return false;
}

bool createNewDir(const std::string& path_name)
{
  if (isDirExist(path_name))
  {
    return true;
  }
  return boost::filesystem::create_directories(path_name);
}

bool isFileExist(const std::string& file_name)
{
  if (boost::filesystem::exists(file_name) && boost::filesystem::is_regular_file(file_name))
  {
    return true;
  }
  return false;
}

bool createNewFile(const std::string& file_name)
{
  if (isFileExist(file_name))
  {
    return true;
  }
  boost::filesystem::ofstream file(file_name);
  file.close();
  return isFileExist(file_name);
}

}  // namespace

namespace moveit_rviz_plugin_scene_editor
{
SceneEditorFrame::SceneEditorFrame(SceneEditorDisplay* pdisplay, rviz::DisplayContext* context, QWidget* parent)
  : QWidget(parent), lock_position_val(0), planning_display_(pdisplay), context_(context), ui_(new Ui::SceneEditorFrame)
{
  ui_->setupUi(this);
  ui_->shapes_combo_box->addItem("Box", shapes::BOX);
  ui_->shapes_combo_box->addItem("Sphere", shapes::SPHERE);
  ui_->shapes_combo_box->addItem("Cylinder", shapes::CYLINDER);
  ui_->shapes_combo_box->addItem("Cone", shapes::CONE);
  ui_->shapes_combo_box->addItem("Mesh from file", shapes::MESH);
  ui_->shapes_combo_box->addItem("Mesh from URL", shapes::MESH);
  setLocalSceneEdited(false);

  connect(ui_->object_x, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
  connect(ui_->object_y, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
  connect(ui_->object_z, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));

  connect(ui_->lock_button_px, SIGNAL(clicked()), this, SLOT(lockPosition()));
  connect(ui_->lock_button_py, SIGNAL(clicked()), this, SLOT(lockPosition()));
  connect(ui_->lock_button_pz, SIGNAL(clicked()), this, SLOT(lockPosition()));
  ui_->lock_button_px->setCheckable(true);
  ui_->lock_button_py->setCheckable(true);
  ui_->lock_button_pz->setCheckable(true);

  connect(ui_->object_rx, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
  connect(ui_->object_ry, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
  connect(ui_->object_rz, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));

  connect(ui_->shape_size_x_spin_box, SIGNAL(valueChanged(double)), this, SLOT(objectSizeValueChanged(double)));
  connect(ui_->shape_size_y_spin_box, SIGNAL(valueChanged(double)), this, SLOT(objectSizeValueChanged(double)));
  connect(ui_->shape_size_z_spin_box, SIGNAL(valueChanged(double)), this, SLOT(objectSizeValueChanged(double)));

  connect(ui_->publish_current_scene_button, SIGNAL(clicked()), this, SLOT(publishScene()));

  connect(ui_->collision_objects_list, SIGNAL(itemSelectionChanged()), this, SLOT(selectedCollisionObjectChanged()));
  connect(ui_->collision_objects_list, SIGNAL(itemChanged(QListWidgetItem*)), this,
          SLOT(collisionObjectChanged(QListWidgetItem*)));
  connect(ui_->collision_objects_list, SIGNAL(customContextMenuRequested(const QPoint&)), this,
          SLOT(onCustomContextMenuRequested(const QPoint&)));

  ui_->collision_objects_list->setStyleSheet(
      "QListWidget::item { background-color: rgb(240, 240, 240); margin: 1px;}"
      "QListWidget:item::selected { background-color: rgb(93, 160, 199); color: white;}");

  ui_->prefab_object_list->setWrapping(true);
  ui_->prefab_object_list->setFlow(QListView::LeftToRight);
  ui_->prefab_object_list->setStyleSheet(
      "QListWidget::item { background-color: rgb(220, 220, 220); margin: 2px;}"
      "QListWidget:item::selected { background-color: rgb(93, 160, 199); color: white;}");

  connect(ui_->add_object_button, &QPushButton::clicked, this, &SceneEditorFrame::addSceneObject);
  connect(ui_->export_scene_geometry_text_button, SIGNAL(clicked()), this, SLOT(exportGeometryAsTextButtonClicked()));
  connect(ui_->import_scene_geometry_text_button, SIGNAL(clicked()), this, SLOT(importGeometryFromTextButtonClicked()));

  QShortcut* copy_object_shortcut = new QShortcut(QKeySequence(Qt::CTRL + Qt::Key_C), ui_->collision_objects_list);
  connect(copy_object_shortcut, SIGNAL(activated()), this, SLOT(copySelectedCollisionObject()));

  ui_->background_job_progress->hide();
  ui_->background_job_progress->setMaximum(0);

  known_collision_objects_version_ = 0;

  planning_scene_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
  planning_scene_world_publisher_ = nh_.advertise<moveit_msgs::PlanningSceneWorld>("planning_scene_world", 1);

  planning_display_->addMainLoopJob(boost::bind(&SceneEditorFrame::loadPrefab, this));
}

SceneEditorFrame::~SceneEditorFrame()
{
  delete ui_;
}

void SceneEditorFrame::enable()
{
  ui_->object_status->setText("");

  // activate the frame
  parentWidget()->show();
}

void SceneEditorFrame::disable()
{
  scene_marker_.reset();
  parentWidget()->hide();
}

void SceneEditorFrame::setLocalSceneEdited(bool dirty)
{
  ui_->publish_current_scene_button->setEnabled(dirty);
}

bool SceneEditorFrame::isLocalSceneDirty() const
{
  return ui_->publish_current_scene_button->isEnabled();
}

void SceneEditorFrame::publishScene()
{
  const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
  if (ps)
  {
    moveit_msgs::PlanningScene msg;
    ps->getPlanningSceneMsg(msg);
    planning_scene_publisher_.publish(msg);
    setLocalSceneEdited(false);
  }
}

void SceneEditorFrame::publishSceneIfNeeded()
{
  if (isLocalSceneDirty() &&
      QMessageBox::question(this, "Update PlanningScene",
                            "You have local changes to your planning scene.\n"
                            "Publish them to the move_group node?",
                            QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes) == QMessageBox::Yes)
    publishScene();
}

void SceneEditorFrame::renameSceneObject()
{
  QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
  if (sel.size() != 1)
    return;

  planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
  if (ps)
  {
    collision_detection::World::ObjectConstPtr obj = ps->getWorld()->getObject(sel[0]->text().toStdString());
    if (obj)
    {
      std::string old_name = obj->id_;
      std::string prompt_str = "场景元素 \"" + old_name + "\" 重命名为:";
      std::string err_str;
      do
      {
        std::string new_name = QInputDialog::getText(this, tr("重命名"), tr((err_str + prompt_str).c_str()),
                                                     QLineEdit::Normal, QString(old_name.c_str()))
                                   .toStdString();

        if (new_name.empty())
          break;

        if (!ps->getWorld()->hasObject(new_name))
        {
          ps->getWorldNonConst()->removeObject(old_name);
          ps->getWorldNonConst()->addToObject(new_name, obj->shapes_, obj->shape_poses_);
          planning_display_->addMainLoopJob(boost::bind(&SceneEditorFrame::populateCollisionObjectsList, this));
          break;
        }
        else
        {
          err_str = "<font style='color:red;'>场景元素 \"" + new_name + "\" 已存在</font><br>";
        }
      } while (true);
    }
  }
}

void SceneEditorFrame::combinationSceneObject()
{
  QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
  if (sel.empty())
    return;

  planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
  if (!ps)
    return;

  unsigned int n = 0;
  std::string group_name = "Group_";
  while (ps->getWorld()->hasObject(group_name + boost::lexical_cast<std::string>(n)))
    n++;
  group_name += boost::lexical_cast<std::string>(n);

  for (const QListWidgetItem* item : sel)
  {
    std::string name = item->text().toStdString();
    collision_detection::World::ObjectConstPtr obj = ps->getWorld()->getObject(name);
    if (!obj)
      continue;

    ps->getWorldNonConst()->removeObject(obj->id_);
    ps->getWorldNonConst()->addToObject(group_name, obj->shapes_, obj->shape_poses_);
    ROS_DEBUG("combination collision object to '%s'", group_name.c_str());
  }
  setLocalSceneEdited();
  planning_display_->addMainLoopJob(boost::bind(&SceneEditorFrame::populateCollisionObjectsList, this));

  // Automatically select the inserted object so that its IM is displayed
  planning_display_->addMainLoopJob(
      boost::bind(&SceneEditorFrame::setItemSelectionInList, this, group_name, true, ui_->collision_objects_list));

  planning_display_->queueRenderSceneGeometry();
}

void SceneEditorFrame::unCombinationSceneObject()
{
  QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
  if (sel.size() != 1)
    return;

  planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
  if (ps)
  {
    collision_detection::World::ObjectConstPtr obj = ps->getWorld()->getObject(sel[0]->text().toStdString());

    if (obj)
    {
      ps->getWorldNonConst()->removeObject(obj->id_);

      for (int i = 0; i < obj->shapes_.size(); ++i)
      {
        static const char* shape_str[8] = { "Unknow", "Sphere", "Cylinder", "Cone", "Box", "Plane", "Mesh", "Octree" };

        auto shape = obj->shapes_[i];
        auto pose = obj->shape_poses_[i];
        unsigned int shape_type = shape->type;

        if (shape_type > (sizeof(shape_str) / sizeof(char*)))
        {
          shape_type = 0;
        }

        unsigned int n = 0;
        std::string shape_name = std::string(shape_str[shape->type]) + "_";
        while (ps->getWorld()->hasObject(shape_name + boost::lexical_cast<std::string>(n)))
          n++;
        shape_name += boost::lexical_cast<std::string>(n);

        ps->getWorldNonConst()->addToObject(shape_name, shape, pose);
      }
    }

    scene_marker_.reset();
    setLocalSceneEdited();
    planning_display_->addMainLoopJob(boost::bind(&SceneEditorFrame::populateCollisionObjectsList, this));
    planning_display_->queueRenderSceneGeometry();
  }
}

void SceneEditorFrame::saveAsPrefabSceneObject()
{
  QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
  if (sel.size() != 1)
    return;

  planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
  if (ps)
  {
    std::stringstream out;

    collision_detection::World::ObjectConstPtr obj = ps->getWorld()->getObject(sel[0]->text().toStdString());
    if (obj)
    {
      auto id = obj->id_;
      out << ps->getName() << std::endl;
      out << "* " << id << std::endl;
      out << obj->shapes_.size() << std::endl;
      for (std::size_t j = 0; j < obj->shapes_.size(); ++j)
      {
        shapes::saveAsText(obj->shapes_[j].get(), out);
        // shape_poses_ is valid isometry by contract
        out << obj->shape_poses_[j].translation().x() << " " << obj->shape_poses_[j].translation().y() << " "
            << obj->shape_poses_[j].translation().z() << std::endl;
        Eigen::Quaterniond r(obj->shape_poses_[j].linear());
        out << r.x() << " " << r.y() << " " << r.z() << " " << r.w() << std::endl;
        if (ps->hasObjectColor(id))
        {
          const std_msgs::ColorRGBA& c = ps->getObjectColor(id);
          out << c.r << " " << c.g << " " << c.b << " " << c.a << std::endl;
        }
        else
          out << "0 0 0 0" << std::endl;
      }
      out << "." << std::endl;

      std::string prefab_dir_path = ros::package::getPath(PACKAGE_NAME) + "/prefab";
      std::string file_path = prefab_dir_path + "/" + id + ".scene";

      if (createNewDir(prefab_dir_path))
      {
        if (!isFileExist(file_path))
        {
          std::ofstream fout(file_path.c_str());
          if (fout.good())
          {
            fout << out.str();
            fout.close();
            planning_display_->addMainLoopJob(boost::bind(&SceneEditorFrame::loadPrefab, this));

            // ROS_INFO("Saved collision object prefab to '%s'", file_path.c_str());
            QMessageBox::information(this, QString("OK !"), QString("Prefab is saved."));
            // auto m_box = new QMessageBox(QMessageBox::Information, QString("OK !"), QString("Prefab is saved."));
            // m_box->setModal(false);
            // QTimer::singleShot(1000, m_box, SLOT(accept()));
            // m_box->exec();
          }
          else
          {
            // ROS_WARN("Unable to save collision object prefab to '%s'", file_path.c_str());
            QMessageBox::warning(this, QString("Sorry !"),
                                 QString("<font style='color:red;'>Prefab is saved failed.</font>"));
          }
        }
        else
        {
          QMessageBox::warning(
              this, QString("Sorry !"),
              QString("<font style='color:red;'>Same Prefab is Exist.<br>Please rename and save again.</font>"));
        }
      }
    }
  }
}

void SceneEditorFrame::addPrefabSceneObject()
{
  planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
  if (!ps)
    return;

  QToolButton* btn = qobject_cast<QToolButton*>(sender());
  if (btn == nullptr)
  {
    return;
  }

  std::string prefab_name = btn->text().toStdString();
  if (prefab_objects_.find(prefab_name) == prefab_objects_.end())
  {
    return;
  }
  auto& obj = prefab_objects_[prefab_name];

  unsigned int n = 0;
  std::string obj_name = prefab_name + "_";
  while (ps->getWorld()->hasObject(obj_name + boost::lexical_cast<std::string>(n)))
    n++;
  obj_name += boost::lexical_cast<std::string>(n);

  ps->getWorldNonConst()->addToObject(obj_name, obj->shapes_, obj->shape_poses_);

  setLocalSceneEdited();
  planning_display_->addMainLoopJob(boost::bind(&SceneEditorFrame::populateCollisionObjectsList, this));
  planning_display_->queueRenderSceneGeometry();
}

void SceneEditorFrame::delPrefabSceneObject()
{
  QToolButton* btn = qobject_cast<QToolButton*>(sender());
  if (btn == nullptr)
  {
    return;
  }

  std::string prefab_file_path = btn->text().toStdString();
  if (!isFileExist(prefab_file_path))
  {
    return;
  }

  if (QMessageBox::question(this, "Del Prefab", "Do have really want to delete this Prefab ?",
                            QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes) == QMessageBox::No)
  {
    return;
  }

  fs::path fs_path = prefab_file_path;
  fs::remove(fs_path);
  loadPrefab();

  //  std::string prefab_name = fs_path.stem().string();
  //  if (prefab_objects_.find(prefab_name) != prefab_objects_.end())
  //  {
  //    prefab_objects_.erase(prefab_name);
  //  }
  //  fs::remove(fs_path);
  //
  //  for (int i = 0; i < ui_->prefab_object_list->count(); ++i)
  //  {
  //    auto item = ui_->prefab_object_list->item(i);
  //    if (item->text() == QString::fromStdString(prefab_name))
  //    {
  //      ui_->prefab_object_list->removeItemWidget(item);
  //      break;
  //    }
  //  }
}

void SceneEditorFrame::onCustomContextMenuRequested(const QPoint& point)
{
  auto list_widget = ui_->collision_objects_list;

  QMenu* pMenu = new QMenu(list_widget);

  QAction* action_clear = new QAction("清除所有", pMenu);
  action_clear->setEnabled(true);
  connect(action_clear, SIGNAL(triggered()), this, SLOT(clearScene()));

  QAction* action_copy = new QAction("复制", pMenu);
  action_copy->setEnabled(true);
  connect(action_copy, SIGNAL(triggered()), this, SLOT(copySelectedCollisionObject()));

  QAction* action_del = new QAction("删除", pMenu);
  action_del->setEnabled(true);
  connect(action_del, SIGNAL(triggered()), this, SLOT(removeSceneObject()));

  QAction* action_rename = new QAction("重命名", pMenu);
  action_rename->setEnabled(false);
  connect(action_rename, SIGNAL(triggered()), this, SLOT(renameSceneObject()));

  QAction* action_combination = new QAction("组合", pMenu);
  action_combination->setEnabled(false);
  connect(action_combination, SIGNAL(triggered()), this, SLOT(combinationSceneObject()));

  QAction* action_uncombination = new QAction("取消组合", pMenu);
  action_uncombination->setEnabled(false);
  connect(action_uncombination, SIGNAL(triggered()), this, SLOT(unCombinationSceneObject()));

  QAction* action_save_as_prefab = new QAction("保存为预制体", pMenu);
  action_save_as_prefab->setEnabled(false);
  connect(action_save_as_prefab, SIGNAL(triggered()), this, SLOT(saveAsPrefabSceneObject()));

  if (list_widget->itemAt(point) == nullptr)
  {
    pMenu->addAction(action_clear);

    if (list_widget->count() == 0)
    {
      action_clear->setEnabled(false);
    }
  }
  else
  {
    pMenu->addAction(action_copy);
    pMenu->addSeparator();
    pMenu->addAction(action_del);
    pMenu->addSeparator();
    pMenu->addAction(action_rename);
    pMenu->addSeparator();
    pMenu->addAction(action_combination);
    pMenu->addAction(action_uncombination);
    pMenu->addSeparator();
    pMenu->addAction(action_save_as_prefab);
    pMenu->addSeparator();
    pMenu->addAction(action_clear);

    // 处理Action使能
    QList<QListWidgetItem*> sel = list_widget->selectedItems();
    if (sel.size() == 1)
    {
      action_rename->setEnabled(true);
      action_save_as_prefab->setEnabled(true);

      planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
      if (ps)
      {
        collision_detection::World::ObjectConstPtr obj = ps->getWorld()->getObject(sel[0]->text().toStdString());
        if (obj && obj->shapes_.size() > 1)
        {
          action_uncombination->setEnabled(true);
        }
      }
    }
    else if (!sel.empty())
    {
      action_combination->setEnabled(true);
    }
  }

  pMenu->exec(ui_->collision_objects_list->mapToGlobal(point));
}

void SceneEditorFrame::clearScene()
{
  planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
  if (ps)
  {
    ps->getWorldNonConst()->clearObjects();
    ps->getCurrentStateNonConst().clearAttachedBodies();
    moveit_msgs::PlanningScene msg;
    ps->getPlanningSceneMsg(msg);
    planning_scene_publisher_.publish(msg);
    setLocalSceneEdited(false);
    planning_display_->addMainLoopJob(boost::bind(&SceneEditorFrame::populateCollisionObjectsList, this));
    planning_display_->queueRenderSceneGeometry();
  }
}

void SceneEditorFrame::removeSceneObject()
{
  QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
  if (sel.empty())
    return;

  planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
  if (ps)
  {
    for (int i = 0; i < sel.count(); ++i)
      ps->getWorldNonConst()->removeObject(sel[i]->text().toStdString());

    scene_marker_.reset();
    setLocalSceneEdited();
    planning_display_->addMainLoopJob(boost::bind(&SceneEditorFrame::populateCollisionObjectsList, this));
    planning_display_->queueRenderSceneGeometry();
  }
}

static QString decideStatusText(const collision_detection::World::ObjectConstPtr& obj)
{
  QString status_text = "'" + QString::fromStdString(obj->id_) + "' is a collision object with ";
  if (obj->shapes_.empty())
    status_text += "no geometry";
  else
  {
    std::vector<QString> shape_names;
    for (const shapes::ShapeConstPtr& shape : obj->shapes_)
      shape_names.push_back(QString::fromStdString(shapes::shapeStringName(shape.get())));
    if (shape_names.size() == 1)
      status_text += "one " + shape_names[0];
    else
    {
      status_text += QString::fromStdString(boost::lexical_cast<std::string>(shape_names.size())) + " shapes:";
      for (const QString& shape_name : shape_names)
        status_text += " " + shape_name;
    }
    status_text += ".";
  }
  return status_text;
}

void SceneEditorFrame::selectedCollisionObjectChanged()
{
  QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
  if (sel.empty())
  {
    bool old_state = ui_->object_x->blockSignals(true);
    ui_->object_x->setValue(0.0);
    ui_->object_x->blockSignals(old_state);

    old_state = ui_->object_y->blockSignals(true);
    ui_->object_y->setValue(0.0);
    ui_->object_y->blockSignals(old_state);

    old_state = ui_->object_z->blockSignals(true);
    ui_->object_z->setValue(0.0);
    ui_->object_z->blockSignals(old_state);

    old_state = ui_->object_rx->blockSignals(true);
    ui_->object_rx->setValue(0.0);
    ui_->object_rx->blockSignals(old_state);

    old_state = ui_->object_ry->blockSignals(true);
    ui_->object_ry->setValue(0.0);
    ui_->object_ry->blockSignals(old_state);

    old_state = ui_->object_rz->blockSignals(true);
    ui_->object_rz->setValue(0.0);
    ui_->object_rz->blockSignals(old_state);

    ui_->object_status->setText("");
    scene_marker_.reset();
    // ui_->pose_scale_group_box->setEnabled(false);
  }
  else if (planning_display_->getPlanningSceneMonitor())
  {
    // ui_->pose_scale_group_box->setEnabled(true);
    bool update_scene_marker = false;
    // Eigen::Isometry3d obj_pose;
    {
      const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
      const collision_detection::World::ObjectConstPtr& obj = ps->getWorld()->getObject(sel[0]->text().toStdString());
      if (obj)
      {
        ui_->object_status->setText(decideStatusText(obj));

        if (obj->shapes_.size() >= 1)
        {
          auto obj_pose = obj->shape_poses_[0];  // valid isometry by contract
          Eigen::Vector3d xyz = obj_pose.linear().eulerAngles(0, 1, 2);
          update_scene_marker = true;  // do the marker update outside locked scope to avoid deadlock

          // 位置
          bool old_state = ui_->object_x->blockSignals(true);
          ui_->object_x->setValue(obj_pose.translation()[0]);
          ui_->object_x->blockSignals(old_state);

          old_state = ui_->object_y->blockSignals(true);
          ui_->object_y->setValue(obj_pose.translation()[1]);
          ui_->object_y->blockSignals(old_state);

          old_state = ui_->object_z->blockSignals(true);
          ui_->object_z->setValue(obj_pose.translation()[2]);
          ui_->object_z->blockSignals(old_state);

          // 旋转
          old_state = ui_->object_rx->blockSignals(true);
          ui_->object_rx->setValue(xyz[0]);
          ui_->object_rx->blockSignals(old_state);

          old_state = ui_->object_ry->blockSignals(true);
          ui_->object_ry->setValue(xyz[1]);
          ui_->object_ry->blockSignals(old_state);

          old_state = ui_->object_rz->blockSignals(true);
          ui_->object_rz->setValue(xyz[2]);
          ui_->object_rz->blockSignals(old_state);

          // 大小
          double size[3] = { 0 };
          switch (obj->shapes_[0]->type)
          {
            case shapes::BOX:
            {
              ui_->shape_size_x_spin_box->setEnabled(true);
              ui_->shape_size_y_spin_box->setEnabled(true);
              ui_->shape_size_z_spin_box->setEnabled(true);

              shapes::Box* shape = (shapes::Box*)(obj->shapes_[0].get());
              size[0] = shape->size[0];
              size[1] = shape->size[1];
              size[2] = shape->size[2];
            }
            break;
            case shapes::SPHERE:
            {
              ui_->shape_size_x_spin_box->setEnabled(true);
              ui_->shape_size_y_spin_box->setEnabled(false);
              ui_->shape_size_z_spin_box->setEnabled(false);

              shapes::Sphere* shape = (shapes::Sphere*)(obj->shapes_[0].get());
              size[0] = 2 * shape->radius;
            }
            break;
            case shapes::CONE:
            {
              ui_->shape_size_x_spin_box->setEnabled(true);
              ui_->shape_size_y_spin_box->setEnabled(false);
              ui_->shape_size_z_spin_box->setEnabled(true);

              shapes::Cone* shape = (shapes::Cone*)(obj->shapes_[0].get());
              size[0] = 2 * shape->radius;
              size[2] = shape->length;
            }
            break;
            case shapes::CYLINDER:
            {
              ui_->shape_size_x_spin_box->setEnabled(true);
              ui_->shape_size_y_spin_box->setEnabled(false);
              ui_->shape_size_z_spin_box->setEnabled(true);

              shapes::Cylinder* shape = (shapes::Cylinder*)(obj->shapes_[0].get());
              size[0] = 2 * shape->radius;
              size[2] = shape->length;
            }
            break;
            case shapes::MESH:
            {
              ui_->shape_size_x_spin_box->setEnabled(false);
              ui_->shape_size_y_spin_box->setEnabled(false);
              ui_->shape_size_z_spin_box->setEnabled(false);

              shapes::Mesh* shape = (shapes::Mesh*)(obj->shapes_[0].get());
              size[0] = 1;
              size[1] = 1;
              size[2] = 1;
            }
            break;
          }

          if (obj->shapes_.size() > 1)
          {
            ui_->shape_size_x_spin_box->setEnabled(false);
            ui_->shape_size_y_spin_box->setEnabled(false);
            ui_->shape_size_z_spin_box->setEnabled(false);
          }

          old_state = ui_->shape_size_x_spin_box->blockSignals(true);
          ui_->shape_size_x_spin_box->setValue(size[0]);
          ui_->shape_size_x_spin_box->blockSignals(old_state);

          old_state = ui_->shape_size_y_spin_box->blockSignals(true);
          ui_->shape_size_y_spin_box->setValue(size[1]);
          ui_->shape_size_y_spin_box->blockSignals(old_state);

          old_state = ui_->shape_size_z_spin_box->blockSignals(true);
          ui_->shape_size_z_spin_box->setValue(size[2]);
          ui_->shape_size_z_spin_box->blockSignals(old_state);
        }
      }
      else
        ui_->object_status->setText("ERROR: '" + sel[0]->text() + "' should be a collision object but it is not");
    }
    if (update_scene_marker)
    {
      createSceneInteractiveMarker();
    }
  }
}

void SceneEditorFrame::lockPosition()
{
  QToolButton* ui_btns[3] = { ui_->lock_button_px, ui_->lock_button_py, ui_->lock_button_pz };

  QToolButton* btn = qobject_cast<QToolButton*>(sender());
  if (btn == nullptr)
  {
    return;
  }

  // 计算 lock_position_val
  for (int i = 0; i < 3; ++i)
  {
    if (btn == ui_btns[i])
    {
      lock_position_val ^= (0x01 << i);  //对应位取反
      break;
    }
  }

  // 切换显示
  for (int i = 0; i < 3; ++i)
  {
    ui_btns[i]->setChecked(lock_position_val & (0x01 << i));
  }
}

void SceneEditorFrame::objectPoseValueChanged(double /* value */)
{
  updateCollisionObjectPose(true);
}

void SceneEditorFrame::objectSizeValueChanged(double /* value */)
{
  updateCollisionObjectSize(true);
}

bool SceneEditorFrame::createPrefabSceneObject(const fs::path& scene_file_path)
{
  std::ifstream in(scene_file_path.c_str());
  std::string obj_name = scene_file_path.stem().string();
  collision_detection::World::ObjectPtr obj = std::make_shared<collision_detection::World::Object>(obj_name);

  if (!in.good() || in.eof())
  {
    return false;
  }

  std::string scene_name;
  std::getline(in, scene_name);
  do
  {
    std::string marker;
    in >> marker;
    if (!in.good() || in.eof())
    {
      return false;
    }
    if (marker == "*")
    {
      std::string ns;
      std::getline(in, ns);
      if (!in.good() || in.eof())
      {
        return false;
      }

      boost::algorithm::trim(ns);
      unsigned int shape_count;
      in >> shape_count;
      for (std::size_t i = 0; i < shape_count && in.good() && !in.eof(); ++i)
      {
        const auto shape = shapes::ShapeConstPtr(shapes::constructShapeFromText(in));
        if (!shape)
        {
          return false;
        }
        double x, y, z, rx, ry, rz, rw;
        if (!(in >> x >> y >> z))
        {
          return false;
        }
        if (!(in >> rx >> ry >> rz >> rw))
        {
          return false;
        }
        float r, g, b, a;
        if (!(in >> r >> g >> b >> a))
        {
          return false;
        }
        if (shape)
        {
          Eigen::Isometry3d pose = Eigen::Translation3d(x, y, z) * Eigen::Quaterniond(rw, rx, ry, rz);

          obj->shapes_.push_back(shape);
          obj->shape_poses_.push_back(pose);
        }
      }
      prefab_objects_[obj_name] = obj;
      return true;
    }
    else if (marker == ".")
    {
      // Marks the end of the scene geometry;
      return true;
    }
    else
    {
      return false;
    }
  } while (true);
}

void SceneEditorFrame::loadPrefab()
{
  std::string prefab_dir_path = ros::package::getPath(PACKAGE_NAME) + "/prefab";

  if (isDirExist(prefab_dir_path))
  {
    std::vector<fs::path> prefab_files;
    fs::directory_iterator end_iter;
    for (fs::directory_iterator file_itr(prefab_dir_path); file_itr != end_iter; ++file_itr)
    {
      if (!fs::is_directory(*file_itr) && (fs::extension(*file_itr) == ".scene"))  // 文件后缀
      {
        prefab_files.push_back(file_itr->path());  //获取文件名
      }
    }

    prefab_objects_.clear();
    ui_->prefab_object_list->clear();
    for (int i = 0; i < prefab_files.size(); ++i)
    {
      const fs::path prefab_file_path = prefab_files[i];
      if (!createPrefabSceneObject(prefab_file_path))
      {
        continue;
      }

      std::string prefab_name = prefab_file_path.stem().string();
      auto item = new QListWidgetItem(QString::fromStdString(prefab_name), ui_->prefab_object_list, i);
      item->setTextAlignment(Qt::Alignment::enum_type::AlignCenter);
      item->setToolTip(decideStatusText(prefab_objects_[prefab_name]));
      item->setSizeHint(QSize(80, 80));

      QWidget* w = new QWidget;

      // 添加预制体到场景按钮
      QToolButton* add_new_btn = new QToolButton(w);
      add_new_btn->setIcon(QIcon(":/icons/add.png"));
      add_new_btn->setStyleSheet("QToolButton {border: 0px;}"
                                 "QToolButton::hover {background: transparent; border: 1px solid rgb(200,200,200);}"
                                 "QToolButton::pressed {background: transparent; border: 1px solid rgb(200,200,200);}");
      add_new_btn->setText(QString::fromStdString(prefab_name));
      connect(add_new_btn, SIGNAL(clicked()), this, SLOT(addPrefabSceneObject()));

      // 删除预制体按钮
      QToolButton* del_prefab_btn = new QToolButton(w);
      del_prefab_btn->setIcon(QIcon(":/icons/remove.png"));
      del_prefab_btn->setStyleSheet(
          "QToolButton {border: 0px;}"
          "QToolButton::hover {background: transparent; border: 1px solid rgb(200,200,200);}"
          "QToolButton::pressed {background: transparent; border: 1px solid rgb(200,200,200);}");
      del_prefab_btn->setText(QString::fromStdString(prefab_file_path.string()));
      connect(del_prefab_btn, SIGNAL(clicked()), this, SLOT(delPrefabSceneObject()));

      QVBoxLayout* layout = new QVBoxLayout;
      layout->setContentsMargins(0, 0, 0, 0);
      layout->addWidget(del_prefab_btn);
      layout->addSpacerItem(new QSpacerItem(20, 40, QSizePolicy::Minimum, QSizePolicy::Expanding));
      layout->addWidget(add_new_btn);
      layout->setAlignment(Qt::Alignment::enum_type::AlignRight);
      w->setLayout(layout);

      ui_->prefab_object_list->addItem(item);
      ui_->prefab_object_list->setItemWidget(item, w);
    }
  }
}

void SceneEditorFrame::updateCollisionObjectPose(bool update_marker_position)
{
  QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
  if (sel.empty())
    return;

  planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
  if (ps)
  {
    collision_detection::World::ObjectConstPtr obj = ps->getWorld()->getObject(sel[0]->text().toStdString());
    if (obj && obj->shapes_.size() >= 1)
    {
      auto start_pose = obj->shape_poses_[0];

      decltype(start_pose) aim_pose;
      aim_pose.translation()[0] = ui_->object_x->value();
      aim_pose.translation()[1] = ui_->object_y->value();
      aim_pose.translation()[2] = ui_->object_z->value();
      aim_pose = Eigen::Translation3d(aim_pose.translation()) *
                 (Eigen::AngleAxisd(ui_->object_rx->value(), Eigen::Vector3d::UnitX()) *
                  Eigen::AngleAxisd(ui_->object_ry->value(), Eigen::Vector3d::UnitY()) *
                  Eigen::AngleAxisd(ui_->object_rz->value(), Eigen::Vector3d::UnitZ()));

      ps->getWorldNonConst()->moveShapeInObject(obj->id_, obj->shapes_[0], aim_pose);

      auto T = aim_pose * start_pose.inverse();
      for (int i = 1; i < obj->shapes_.size(); ++i)
      {
        ps->getWorldNonConst()->moveShapeInObject(obj->id_, obj->shapes_[i], T * obj->shape_poses_[i]);
      }

      planning_display_->queueRenderSceneGeometry();
      setLocalSceneEdited();

      // Update the interactive marker pose to match the manually introduced one
      if (update_marker_position && scene_marker_)
      {
        // p is isometry by construction
        Eigen::Quaterniond eq(aim_pose.linear());
        scene_marker_->setPose(Ogre::Vector3(ui_->object_x->value(), ui_->object_y->value(), ui_->object_z->value()),
                               Ogre::Quaternion(eq.w(), eq.x(), eq.y(), eq.z()), "");
      }
    }
  }
}

void SceneEditorFrame::updateCollisionObjectSize(bool update_marker_position)
{
  QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
  if (sel.empty())
    return;
  planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
  if (ps)
  {
    collision_detection::World::ObjectConstPtr obj = ps->getWorld()->getObject(sel[0]->text().toStdString());
    if (obj && obj->shapes_.size() == 1)
    {
      double x_length = ui_->shape_size_x_spin_box->value();
      double y_length = ui_->shape_size_y_spin_box->value();
      double z_length = ui_->shape_size_z_spin_box->value();
      if (x_length < MIN_VAL || y_length < MIN_VAL || z_length < MIN_VAL)
      {
        QMessageBox::warning(this, QString("Dimension is too small"),
                             QString("Size values need to be >= %1").arg(MIN_VAL));
        return;
      }

      switch (obj->shapes_[0]->type)
      {
        case shapes::BOX:
        {
          shapes::Box* shape = (shapes::Box*)(obj->shapes_[0].get());
          shape->size[0] = x_length;
          shape->size[1] = y_length;
          shape->size[2] = z_length;
        }
        break;
        case shapes::SPHERE:
        {
          shapes::Sphere* shape = (shapes::Sphere*)(obj->shapes_[0].get());
          shape->radius = 0.5 * x_length;
        }
        break;
        case shapes::CONE:
        {
          shapes::Cone* shape = (shapes::Cone*)(obj->shapes_[0].get());
          shape->radius = 0.5 * x_length;
          shape->length = z_length;
        }
        break;
        case shapes::CYLINDER:
        {
          shapes::Cylinder* shape = (shapes::Cylinder*)(obj->shapes_[0].get());
          shape->radius = 0.5 * x_length;
          shape->length = z_length;
        }
        break;
        case shapes::MESH:
        {
          shapes::Mesh* shape = (shapes::Mesh*)(obj->shapes_[0].get());
          shape->scale(x_length, y_length, z_length);
        }
        break;
      }
      scene_marker_->processMessage(createObjectMarkerMsg(obj));
      planning_display_->queueRenderSceneGeometry();
      setLocalSceneEdited();
    }
  }
}

void SceneEditorFrame::collisionObjectChanged(QListWidgetItem* item)
{
  if (item->type() < (int)known_collision_objects_.size() && planning_display_->getPlanningSceneMonitor())
  {
    // if we have a name change
    if (known_collision_objects_[item->type()].first != item->text().toStdString())
      renameCollisionObject(item);
  }
}

/* Receives feedback from the interactive marker and updates the shape pose in the world accordingly */
void SceneEditorFrame::imProcessFeedback(visualization_msgs::InteractiveMarkerFeedback& feedback)
{
  bool old_state;
  if (!lock_position_x)
  {
    old_state = ui_->object_x->blockSignals(true);
    ui_->object_x->setValue(feedback.pose.position.x);
    ui_->object_x->blockSignals(old_state);
  }
  if (!lock_position_y)
  {
    old_state = ui_->object_y->blockSignals(true);
    ui_->object_y->setValue(feedback.pose.position.y);
    ui_->object_y->blockSignals(old_state);
  }
  if (!lock_position_z)
  {
    old_state = ui_->object_z->blockSignals(true);
    ui_->object_z->setValue(feedback.pose.position.z);
    ui_->object_z->blockSignals(old_state);
  }

  Eigen::Quaterniond q;
  tf2::fromMsg(feedback.pose.orientation, q);
  Eigen::Vector3d xyz = q.matrix().eulerAngles(0, 1, 2);

  old_state = ui_->object_rx->blockSignals(true);
  ui_->object_rx->setValue(xyz[0]);
  ui_->object_rx->blockSignals(old_state);

  old_state = ui_->object_ry->blockSignals(true);
  ui_->object_ry->setValue(xyz[1]);
  ui_->object_ry->blockSignals(old_state);

  old_state = ui_->object_rz->blockSignals(true);
  ui_->object_rz->setValue(xyz[2]);
  ui_->object_rz->blockSignals(old_state);

  updateCollisionObjectPose(true);
}

void SceneEditorFrame::copySelectedCollisionObject()
{
  QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
  if (sel.empty())
    return;

  planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
  if (!ps)
    return;

  for (const QListWidgetItem* item : sel)
  {
    std::string name = item->text().toStdString();
    collision_detection::World::ObjectConstPtr obj = ps->getWorld()->getObject(name);
    if (!obj)
      continue;

    // find a name for the copy
    name = "Copy of " + name;
    if (ps->getWorld()->hasObject(name))
    {
      name += " ";
      unsigned int n = 1;
      while (ps->getWorld()->hasObject(name + boost::lexical_cast<std::string>(n)))
        n++;
      name += boost::lexical_cast<std::string>(n);
    }
    ps->getWorldNonConst()->addToObject(name, obj->shapes_, obj->shape_poses_);
    ROS_DEBUG("Copied collision object to '%s'", name.c_str());
  }
  setLocalSceneEdited();
  planning_display_->addMainLoopJob(boost::bind(&SceneEditorFrame::populateCollisionObjectsList, this));
  planning_display_->queueRenderSceneGeometry();
}

visualization_msgs::InteractiveMarker
SceneEditorFrame::createObjectMarkerMsg(const collision_detection::World::ObjectConstPtr& obj)
{
  Eigen::Vector3d center;
  double scale_0;
  shapes::computeShapeBoundingSphere(obj->shapes_[0].get(), center, scale_0);
  geometry_msgs::PoseStamped shape_pose = tf2::toMsg(tf2::Stamped<Eigen::Isometry3d>(
      obj->shape_poses_[0], ros::Time(), planning_display_->getRobotModel()->getModelFrame()));
  double scale = (scale_0 + center.cwiseAbs().maxCoeff()) * 2.0 * 1.2;  // add padding of 20% size

  // create an interactive marker msg for the given shape
  visualization_msgs::InteractiveMarker imarker =
      robot_interaction::makeEmptyInteractiveMarker("marker_scene_object", shape_pose, scale);
  std_msgs::ColorRGBA rgba;
  rgba.r = 0;
  rgba.g = 1;
  rgba.b = 1;
  rgba.a = 0.5;
  robot_interaction::addViewPlaneControl(imarker, scale_0, rgba, true, false);
  robot_interaction::addPositionControl(imarker);
  imarker.description = obj->id_;
  interactive_markers::autoComplete(imarker);
  return imarker;
}

void SceneEditorFrame::createSceneInteractiveMarker()
{
  QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
  if (sel.empty())
    return;

  const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
  if (!ps)
    return;

  const collision_detection::World::ObjectConstPtr& obj = ps->getWorld()->getObject(sel[0]->text().toStdString());
  if (obj && obj->shapes_.size() >= 1)  // fan mask todo:
  {
    scene_marker_ = std::make_shared<rviz::InteractiveMarker>(planning_display_->getSceneNode(), context_);
    scene_marker_->processMessage(createObjectMarkerMsg(obj));
    scene_marker_->setShowAxes(false);

    // Connect signals
    connect(scene_marker_.get(), SIGNAL(userFeedback(visualization_msgs::InteractiveMarkerFeedback&)), this,
            SLOT(imProcessFeedback(visualization_msgs::InteractiveMarkerFeedback&)));
  }
  else
  {
    scene_marker_.reset();
  }
}

void SceneEditorFrame::renameCollisionObject(QListWidgetItem* item)
{
  long unsigned int version = known_collision_objects_version_;
  if (item->text().isEmpty())
  {
    QMessageBox::warning(this, "Invalid object name", "Cannot set an empty object name.");
    if (version == known_collision_objects_version_)
      item->setText(QString::fromStdString(known_collision_objects_[item->type()].first));
    return;
  }

  std::string item_text = item->text().toStdString();
  bool already_exists = planning_display_->getPlanningSceneRO()->getWorld()->hasObject(item_text);
  if (!already_exists)
    already_exists = planning_display_->getPlanningSceneRO()->getCurrentState().hasAttachedBody(item_text);
  if (already_exists)
  {
    QMessageBox::warning(this, "Duplicate object name",
                         QString("The name '")
                             .append(item->text())
                             .append("' already exists. Not renaming object ")
                             .append((known_collision_objects_[item->type()].first.c_str())));
    if (version == known_collision_objects_version_)
      item->setText(QString::fromStdString(known_collision_objects_[item->type()].first));
    return;
  }

  planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
  collision_detection::World::ObjectConstPtr obj =
      ps->getWorld()->getObject(known_collision_objects_[item->type()].first);
  if (obj)
  {
    known_collision_objects_[item->type()].first = item_text;
    // const moveit::core::FixedTransformsMap subframes = obj->subframe_poses_;  // Keep subframes
    // TODO(felixvd): Scale the subframes with the object
    ps->getWorldNonConst()->removeObject(obj->id_);
    ps->getWorldNonConst()->addToObject(known_collision_objects_[item->type()].first, obj->shapes_, obj->shape_poses_);
    // ps->getWorldNonConst()->setSubframesOfObject(obj->id_, subframes);
    if (scene_marker_)
    {
      scene_marker_.reset();
      planning_display_->addMainLoopJob(boost::bind(&SceneEditorFrame::createSceneInteractiveMarker, this));
    }
  }
  setLocalSceneEdited();
}

void SceneEditorFrame::populateCollisionObjectsList()
{
  ui_->collision_objects_list->setUpdatesEnabled(false);
  bool old_state = ui_->collision_objects_list->blockSignals(true);
  bool octomap_in_scene = false;

  {
    QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
    std::set<std::string> to_select;
    for (QListWidgetItem* item : sel)
      to_select.insert(item->text().toStdString());
    ui_->collision_objects_list->clear();
    known_collision_objects_.clear();
    known_collision_objects_version_++;

    planning_scene_monitor::LockedPlanningSceneRO ps = planning_display_->getPlanningSceneRO();
    if (ps)
    {
      const std::vector<std::string>& collision_object_names = ps->getWorld()->getObjectIds();
      for (std::size_t i = 0; i < collision_object_names.size(); ++i)
      {
        if (collision_object_names[i] == planning_scene::PlanningScene::OCTOMAP_NS)
        {
          octomap_in_scene = true;
          continue;
        }

        QListWidgetItem* item =
            new QListWidgetItem(QString::fromStdString(collision_object_names[i]), ui_->collision_objects_list, (int)i);
        item->setFlags(item->flags() | Qt::ItemIsEditable);
        item->setToolTip(item->text());

        if (to_select.find(collision_object_names[i]) != to_select.end())
          item->setSelected(true);
        ui_->collision_objects_list->addItem(item);
        known_collision_objects_.push_back(std::make_pair(collision_object_names[i], false));
      }
    }
  }

  ui_->collision_objects_list->blockSignals(old_state);
  ui_->collision_objects_list->setUpdatesEnabled(true);
  selectedCollisionObjectChanged();
}

void SceneEditorFrame::exportGeometryAsTextButtonClicked()
{
  QString path =
      QFileDialog::getSaveFileName(this, tr("Export Scene Geometry"), tr(""), tr("Scene Geometry (*.scene)"));
  if (!path.isEmpty())
    planning_display_->addBackgroundJob(
        boost::bind(&SceneEditorFrame::computeExportGeometryAsText, this, path.toStdString()), "export as text");
}

void SceneEditorFrame::computeExportGeometryAsText(const std::string& path)
{
  planning_scene_monitor::LockedPlanningSceneRO ps = planning_display_->getPlanningSceneRO();
  if (ps)
  {
    std::string p = (path.length() < 7 || path.substr(path.length() - 6) != ".scene") ? path + ".scene" : path;
    std::ofstream fout(p.c_str());
    if (fout.good())
    {
      ps->saveGeometryToStream(fout);
      fout.close();
      ROS_INFO("Saved current scene geometry to '%s'", p.c_str());
    }
    else
      ROS_WARN("Unable to save current scene geometry to '%s'", p.c_str());
  }
}

void SceneEditorFrame::computeImportGeometryFromText(const std::string& path)
{
  planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
  if (ps)
  {
    std::ifstream fin(path.c_str());
    if (ps->loadGeometryFromStream(fin))
    {
      ROS_INFO("Loaded scene geometry from '%s'", path.c_str());
      planning_display_->addMainLoopJob(boost::bind(&SceneEditorFrame::populateCollisionObjectsList, this));
      planning_display_->queueRenderSceneGeometry();
      setLocalSceneEdited();
    }
    else
    {
      QMessageBox::warning(nullptr, "Loading scene geometry",
                           "Failed to load scene geometry.\n"
                           "See console output for more details.");
    }
  }
}

void SceneEditorFrame::importGeometryFromTextButtonClicked()
{
  QString path =
      QFileDialog::getOpenFileName(this, tr("Import Scene Geometry"), tr(""), tr("Scene Geometry (*.scene)"));
  if (!path.isEmpty())
    planning_display_->addBackgroundJob(
        boost::bind(&SceneEditorFrame::computeImportGeometryFromText, this, path.toStdString()), "import from text");
}
void SceneEditorFrame::addSceneObject()
{
  if (!planning_display_->getPlanningSceneMonitor())
  {
    return;
  }

  double x_length = 0.2;
  double y_length = 0.2;
  double z_length = 0.2;

  // by default, name object by shape type
  std::string selected_shape = ui_->shapes_combo_box->currentText().toStdString();
  shapes::ShapeConstPtr shape;
  switch (ui_->shapes_combo_box->currentData().toInt())  // fetch shape ID from current combobox item
  {
    case shapes::BOX:
      shape = std::make_shared<shapes::Box>(x_length, y_length, z_length);
      break;
    case shapes::SPHERE:
      shape = std::make_shared<shapes::Sphere>(0.5 * x_length);
      break;
    case shapes::CONE:
      shape = std::make_shared<shapes::Cone>(0.5 * x_length, z_length);
      break;
    case shapes::CYLINDER:
      shape = std::make_shared<shapes::Cylinder>(0.5 * x_length, z_length);
      break;
    case shapes::MESH:
    {
      QUrl url;
      if (ui_->shapes_combo_box->currentText().contains("file"))  // open from file
        url = QFileDialog::getOpenFileUrl(this, tr("Import Object Mesh"), QString(),
                                          "CAD files (*.stl *.obj *.dae);;All files (*.*)");
      else  // open from URL
        url = QInputDialog::getText(this, tr("Import Object Mesh"), tr("URL for file to import from:"),
                                    QLineEdit::Normal, QString("http://"));
      if (!url.isEmpty())
        shape = loadMeshResource(url.toString().toStdString());
      if (!shape)
        return;
      // name mesh objects by their file name
      selected_shape = url.fileName().toStdString();
      break;
    }
    default:
      QMessageBox::warning(this, QString("Unsupported shape"),
                           QString("The '%1' is not supported.").arg(ui_->shapes_combo_box->currentText()));
  }

  // find available (initial) name of object
  int idx = 0;
  std::string shape_name = selected_shape + "_" + std::to_string(idx);
  while (planning_display_->getPlanningSceneRO()->getWorld()->hasObject(shape_name))
  {
    idx++;
    shape_name = selected_shape + "_" + std::to_string(idx);
  }

  // Actually add object to the plugin's PlanningScene
  {
    planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
    ps->getWorldNonConst()->addToObject(shape_name, shape, Eigen::Isometry3d::Identity());
  }
  setLocalSceneEdited();

  planning_display_->addMainLoopJob(boost::bind(&SceneEditorFrame::populateCollisionObjectsList, this));

  // Automatically select the inserted object so that its IM is displayed
  planning_display_->addMainLoopJob(
      boost::bind(&SceneEditorFrame::setItemSelectionInList, this, shape_name, true, ui_->collision_objects_list));

  planning_display_->queueRenderSceneGeometry();
}

void SceneEditorFrame::setItemSelectionInList(const std::string& item_name, bool selection, QListWidget* list)
{
  QList<QListWidgetItem*> found_items = list->findItems(QString(item_name.c_str()), Qt::MatchExactly);
  for (QListWidgetItem* found_item : found_items)
    found_item->setSelected(selection);
}

shapes::ShapePtr SceneEditorFrame::loadMeshResource(const std::string& url)
{
  shapes::Mesh* mesh = shapes::createMeshFromResource(url);
  if (mesh)
  {
    // If the object is very large, ask the user if the scale should be reduced.
    bool object_is_very_large = false;
    for (unsigned int i = 0; i < mesh->vertex_count; i++)
    {
      if ((abs(mesh->vertices[i * 3 + 0]) > LARGE_MESH_THRESHOLD) ||
          (abs(mesh->vertices[i * 3 + 1]) > LARGE_MESH_THRESHOLD) ||
          (abs(mesh->vertices[i * 3 + 2]) > LARGE_MESH_THRESHOLD))
      {
        object_is_very_large = true;
        break;
      }
    }
    if (object_is_very_large)
    {
      QMessageBox msg_box;
      msg_box.setText(
          "The object is very large (greater than 10 m). The file may be in millimeters instead of meters.");
      msg_box.setInformativeText("Attempt to fix the size by shrinking the object?");
      msg_box.setStandardButtons(QMessageBox::Yes | QMessageBox::No);
      msg_box.setDefaultButton(QMessageBox::Yes);
      if (msg_box.exec() == QMessageBox::Yes)
      {
        for (unsigned int i = 0; i < mesh->vertex_count; ++i)
        {
          unsigned int i3 = i * 3;
          mesh->vertices[i3] *= 0.001;
          mesh->vertices[i3 + 1] *= 0.001;
          mesh->vertices[i3 + 2] *= 0.001;
        }
      }
    }

    return shapes::ShapePtr(mesh);
  }
  else
  {
    QMessageBox::warning(this, QString("Import error"), QString("Unable to import object"));
    return shapes::ShapePtr();
  }
}

void SceneEditorFrame::sceneUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
{
  if (update_type & planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY)
    planning_display_->addMainLoopJob(std::bind(&SceneEditorFrame::populateCollisionObjectsList, this));
}

void SceneEditorFrame::updateSceneMarkers(float wall_dt, float /*ros_dt*/)
{
  // fan mask todo:
  //  if (scene_marker_)
  //    scene_marker_->update(wall_dt);
}

}  // namespace moveit_rviz_plugin_scene_editor
